import numpy as np
import calfem.geometry as cfg
import calfem.mesh as cfm
import calfem.vis as cfv
import numpy as np
from toolkits import gauss_line_int, gauss_tri_int
class Poisson2D:
    """docstring for Poisson
    
    3-Node triangular element FEM/EFEM model for 2D Poisson/Laplace equation. 

    """
    def __init__(self, geometry, DirMarker, NeuMarker, holeMarker, BCfunc):
        self.Geometry = geometry

        self.Node = None
        self.Edge = dict()
        self.Element = None

        self.DirMarker = DirMarker # list of markers for Dirichlet's boundary
        self.Dirichlet = dict()    # (i,j): marker
        # self.DirEdge = dict()
        self.NeuMarker = NeuMarker # list of markers for Neumann's boundary
        self.Neumann = dict()      # (i,j): marker
        # self.NeuEdge = dict()
        self.NeuCluster = None
        self.holeMarker = holeMarker  # list of marker lists for each hole 
        self.hole = dict()      # (i,j): holeid 
        self.holeEdge = dict()  # holeid : [(i,j),(m,n),...]

        self.crossEdge = dict()
        self.crossElement = dict()

        self.BCfunc = BCfunc

        self.numHole = len(self.holeMarker)
        self.numElement = None
        self.numNode = None
        self.numDOF = None

        self.area = None
        # EFEM matrix vector
        self.FU = None
        self.dU = None
        self.F = None
        self.d = None
        self.PhiFe = None
        self.s = None
        self.qh = None
        # FEM matrix vector

        self._mesh = None
        self._edof = None
        self._BdElements = None
      
    def mesh(self, showMesh=False):
        # generate mesh using GMSH.
        self._mesh = cfm.GmshMesh(self.Geometry, return_boundary_elements=True)
        self._mesh.elType = 2
        self._mesh.dofsPerNode = 1
        ##########################################
        #                                        #
        #          Basic Data Structure          #
        #                                        #
        # Node: list of coordinates (x, y)       #
        # Element: list of nodes (i,j,k) CCW.    #
        # Edge: dict of edges (i,j):n pair       #
        #                                        #
        ##########################################
        self.Node, self._edof, _, _, _, self._BdElements = self._mesh.create()
        self.Element = self._edof - 1
        for n, e in enumerate(self.Element):
            self.Edge[(e[0], e[1])] = n
            self.Edge[(e[1], e[2])] = n
            self.Edge[(e[2], e[0])] = n
        self.numElement = len(self.Element)
        self.numNode = len(self.Node)
        self.numDOF = self.numNode + self.numHole
        ###############################################
        #                                             #
        #                 Marker Bank                 #
        #              (for applying BC)              #
        #                                             #
        # DirEdge: dict of marker, marker:(i,j)       #
        # NeuEdge: dict of marker, marker:(i,j)       #
        # holeEdge: dict of holeid, holeid:[(i,j),..] #
        #                                             #
        ###############################################
        # for marker in self.DirMarker:
        #     self.DirEdge[marker] = np.array([e['node-number-list'] for e in self._BdElements[marker]])-1
        
        # for marker in self.NeuMarker:
        #     self.NeuEdge[marker] = np.array([e['node-number-list'] for e in self._BdElements[marker]])-1

        for i, hole in enumerate(self.holeMarker):
            edges = []
            for marker in hole:
                for e in self._BdElements[marker]:
                    edge = tuple(np.array(e['node-number-list'])-1)
                    # Force to CCW.
                    if edge not in self.Edge:
                        edge = edge[::-1]
                    edges.append(edge)
            self.holeEdge[i] = np.array(edges)

        ##############################################
        #                                            #
        #                Boundary sets               #
        #          (for applying constraint)         #
        #                                            #
        # Dirichlet: Dirichlet edge set.(i,j):marker #
        # Neumann: Neumann edge set. (i,j):marker    #
        # hole: hole edge dict. (i,j):holeid         #  
        #                                            #
        ##############################################
        for marker in self.DirMarker:
            for e in self._BdElements[marker]:
                edge = tuple(np.array(e['node-number-list'])-1)
                # Force to CCW.
                if edge not in self.Edge:
                    edge = edge[::-1]
                self.Dirichlet[edge] = marker

        for marker in self.NeuMarker:
            for e in self._BdElements[marker]:
                edge = tuple(np.array(e['node-number-list'])-1)
                # Force to CCW.
                if edge not in self.Edge:
                    edge = edge[::-1]
                self.Neumann[edge] = marker                

        self.NeuCluster = self._cluster_edge(self.Neumann)

        # self._check_ccw()

        if len(self.holeMarker) != 0: 
            for n, hms in enumerate(self.holeMarker):
                for m in hms:
                    for e in self._BdElements[m]:
                        edge = tuple(np.array(e['node-number-list'])-1)
                        # Force to CCW.
                        if edge not in self.Edge:
                            edge = edge[::-1]
                        self.hole[edge] = n
            self._connect_domain()
        # Display mesh
        if showMesh:
            cfv.draw_mesh(coords=self.Node, edof=self._edof, dofs_per_node=self._mesh.dofs_per_node,
                          el_type=self._mesh.el_type,filled=True,title="Mesh")
            cfv.showAndWait()

    def _cluster_edge(self, edges):
        from collections import deque
        from copy import deepcopy

        edges = deepcopy(edges)
        clusters = []

        e, _ = edges.popitem()
        clusters.append(deque([e]))
        
        while len(edges) != 0:
            quit = False
            for i, j in edges:
                for c in clusters:
                    if i == c[-1][-1]:
                        c.append((i, j))
                        edges.pop((i, j), None)
                        quit = True
                        break
                    if j == c[0][0]:
                        c.appendleft((i, j))
                        edges.pop((i, j), None)
                        quit = True
                        break
                if quit:
                    break
            else:
                if len(edges) != 0:
                    e, _ = edges.popitem()
                    clusters.append(deque([e]))
                    
        # convert to list
        clusters = [list(q) for q in clusters]
        return clusters 

    def _check_ccw(self):
        import matplotlib.pyplot as plt
        plt.figure(figsize=(20,10),dpi=300)
        plt.scatter(self.Node[:,0], self.Node[:,1])
        for i,j in self.Neumann:
            x1,y1 = self.Node[i]
            x2,y2 = self.Node[j]
            plt.arrow(x1,y2,x2-x1,y2-y1)
        plt.savefig('ccw.jpg')
        plt.show()

    def _connect_domain(self):
        #################################
        #                               #
        #  Crossing elements and edges  #
        #                               #
        # crossEdge: (i,j):(n,eta)^T    #
        #                               #
        #################################

        direction = np.array([0, -1])

        for n in self.holeEdge:
            edge = tuple(self.holeEdge[n][0])
            self.crossEdge[edge] = np.array([[n+self.numNode], [-1]])

            _connected = False

            while not _connected:
                # print('current:', edge, self.Node[list(edge)][0], self.Node[list(edge)][1])
                ei = self.Edge[edge]
                
                i, j, k = self.Element[ei]
                order = [i,j,k]  
                
                local_edges = set([(i,j), (j,k), (k,i)])
                local_edges.remove(edge)
                e1 = np.array(local_edges.pop())
                e2 = np.array(local_edges.pop())
                
                e1n = self.Node[e1[1]] - self.Node[e1[0]]
                e1n = e1n/np.linalg.norm(e1n)
                
                e2n = self.Node[e2[1]] - self.Node[e2[0]]
                e2n = e2n/np.linalg.norm(e2n)
                
                d1 = np.abs(np.dot(direction, e1n))
                d2 = np.abs(np.dot(direction, e2n))
                ae = np.zeros((3,1))
                if d1 < d2:
                    L1 = np.linalg.norm(self.Node[edge[0]] - self.Node[edge[1]])
                    L2 = np.linalg.norm(self.Node[e1[0]] - self.Node[e1[1]])
                    ae[order.index(edge[0])]= 1/L1
                    ae[order.index(e1[0])]= -1/L2
                    edge = tuple(e1)
                else:
                    L1 = np.linalg.norm(self.Node[edge[0]] - self.Node[edge[1]])
                    L2 = np.linalg.norm(self.Node[e2[0]] - self.Node[e2[1]])
                    ae[order.index(edge[0])]= 1/L1
                    ae[order.index(e2[0])]= -1/L2
                    edge = tuple(e2)

                ae = np.insert(ae, 0, n+self.numNode, axis=0)
                # print('Element={}, ae={}, next <{},{}>'.format(ei, ae[1::].T, *edge))
                if ei in self.crossElement:
                    self.crossElement[ei] = np.c_[self.crossElement[ei], ae]
                else:
                    self.crossElement[ei] = ae
                # print(e1,'\t',e1n)
                # print(e2,'\t',e2n)
                # print(ae)
                # print(d1,d2)
                # print("----------------------------------")
                # print('next:', edge, self.Node[list(edge)][0], self.Node[list(edge)][1])
                # print(self.Neumann.keys())
                if (edge in self.Dirichlet) or (edge in self.Neumann):
                    if edge in self.hole:
                        m = self.hole[edge]
                        if m == n:
                            # TODO: reject.
                            pass
                        else:
                            if edge in self.crossEdge:
                                self.crossEdge[edge] = np.c_[self.crossEdge[edge], np.array([[n+self.numNode], [-1]])]
                            else:
                                self.crossEdge[edge] = np.array([[n+self.numNode], [-1]])
                    else:
                        if edge in self.crossEdge:
                            self.crossEdge[edge] = np.c_[self.crossEdge[edge], np.array([[n+self.numNode], [1]])]
                        else:
                            self.crossEdge[edge] = np.array([[n+self.numNode], [1]])

                    _connected = True
                else:
                    edge = edge[::-1]
                    # print(self.Node[list(edge)])

    def _assembly_efem(self):
        ######################################
        #                                    #
        #  unconstrained flexibility matrix  #
        #                                    #
        ######################################
        self.FU = np.zeros((self.numDOF, self.numDOF))
        self.PhiFe = np.zeros((self.numElement, 2, 3))
        self.area = np.zeros(self.numElement)

        for i, e in enumerate(self.Element):
            x1, y1 = self.Node[e][0]
            x2, y2 = self.Node[e][1]
            x3, y3 = self.Node[e][2]
            
            self.area[i] = 1/2*np.linalg.det(np.array([[x1, y1, 1],
                                                       [x2, y2, 1],
                                                       [x3, y3, 1]]))
            
            self.PhiFe[i] = np.array([[x2-x3, x3-x1, x1-x2],
                                      [y2-y3, y3-y1, y1-y2]])/(2*self.area[i])
            

            if i in self.crossElement:
                L1 = np.sqrt((x2-x1)**2+(y2-y1)**2)
                L2 = np.sqrt((x3-x2)**2+(y3-y2)**2)

                left = np.linalg.inv([[(y1-y2)/L1, (x2-x1)/L1],
                                      [(y2-y3)/L2, (x3-x2)/L2]])

                PhiLe = np.c_[left, np.zeros((2,1))]
                
                # print(Eh[i])
                PhiFe = np.c_[self.PhiFe[i], PhiLe @ self.crossElement[i][1::,:]]
                Fe = self.area[i] * PhiFe.T @ PhiFe
                
                ### Todo: multiple holes ###
                ey = np.r_[e, self.crossElement[i][0,:]].astype(int)
                self.FU[np.ix_(ey,ey)] = self.FU[np.ix_(ey,ey)] + Fe
                # print(i, ey)
            else:
                Fe = self.area[i] * self.PhiFe[i].T @ self.PhiFe[i]
                
                self.FU[np.ix_(e,e)] = self.FU[np.ix_(e,e)] + Fe

        print('size: {} x {}'.format(*self.FU.shape))
        print('rank: {}'.format(np.linalg.matrix_rank(self.FU)))

        ######################################
        #                                    #
        #   unconstrained kinematic vector   #
        #                                    #
        ######################################
        self.dU = np.zeros(self.numDOF)
        #========================#
        #   Dirichlet boundary   #
        #========================#
        for edge in self.Dirichlet:
            marker = self.Dirichlet[edge]
            i, j = edge
            L = np.linalg.norm(self.Node[i]-self.Node[j])
            up = gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
            self.dU[i] = self.dU[i] - up/L
            self.dU[j] = self.dU[j] + up/L

        # for marker in self.DirEdge:
        #     for i,j in self.DirEdge[marker]:
        #         L = np.linalg.norm(self.Node[i]-self.Node[j])
        #         up = gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
        #         self.dU[i] = self.dU[i] - up/L
        #         self.dU[j] = self.dU[j] + up/L

    def _constraint(self):
        #========================================#
        #  1.  Neumann boundary (without source) #
        #========================================#
        self.sN = np.zeros(self.numDOF)
        self.A = np.eye(self.numDOF)

        # todo : move to subfunction.
        eff_dofs = []
        red_dofs = []
        for clusters in self.NeuCluster:
            eff_dofs.append([clusters[0][0]])
            _red = []
            for edge in clusters:
                # to avoid error occured in closed-edge.
                if edge[1] != eff_dofs[-1][0]:
                    _red.append(edge[1])
            red_dofs.append(_red)


        for n, d in enumerate(eff_dofs):
            self.A[np.ix_(red_dofs[n], d)] = 1

        all_red_dofs = [y for x in red_dofs for y in x]

        # Neumann terms
        for cluster in self.NeuCluster:
            rn = {}
            fl_bar = 0
            # exclude the last edge in closed edge.
            if cluster[0][0] == cluster[-1][-1]:
                cn = cluster[:-1]
            else:
                cn = cluster
            for edge in cn:
                marker = self.Neumann[edge]
                i, j = edge
                fl_bar = fl_bar + gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
                self.sN[j] = fl_bar

                if edge in self.crossEdge:
                    dofs, etas = self.crossEdge[edge]
                    for dof, eta in zip(dofs, etas):
                        rn[int(dof)] = eta

                for dof in rn:
                    self.A[j, dof] = rn[dof]

        self.A = np.delete(self.A, all_red_dofs, axis=1)

        self.F = self.A.T @ self.FU @ self.A
        self.d = self.A.T @ (self.dU - self.FU @ self.sN)

        #==============================================#
        #  2.  Uniqueness of nodal variables (s0 = 0)  #
        #==============================================#
        self.F[0,:] = 0
        self.F[0,0] = 1
        self.d[0] = 0

        #=====================================#
        #  3.  Neumann hole (rk is enforced)  #
        #=====================================#
        for cluster in self.NeuCluster:
            # 封闭的一定是孔吗？
            if cluster[0][0] == cluster[-1][-1]:
                holeid = self.hole[cluster[0]]
                dof = -(holeid + 1)
                r = 0
                for edge in cluster:
                    marker = self.Neumann[edge]
                    i, j = edge
                    r = r + gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
                print(dof, r)
                self.F[dof,:] = 0
                self.F[dof,dof] = 1
                self.d[dof] = r

    def _solve_efem(self):
        _s = np.linalg.inv(self.F) @ self.d
        self.s = self.A @ _s + self.sN

    def _construct_flux(self):
        self.qh = np.zeros((self.numElement, 2)) 

        for i, e in enumerate(self.Element):
            
            if i in self.crossElement:
                x1,y1 = self.Node[e][0]
                x2,y2 = self.Node[e][1]
                x3,y3 = self.Node[e][2]
                L1 = np.sqrt((x2-x1)**2+(y2-y1)**2)
                L2 = np.sqrt((x3-x2)**2+(y3-y2)**2)

                left = np.linalg.inv([[(y1-y2)/L1,(x2-x1)/L1],
                                      [(y2-y3)/L2,(x3-x2)/L2]])

                PhiLe = np.c_[left, np.zeros((2,1))]
                
                PhiFe = np.c_[self.PhiFe[i], PhiLe @ self.crossElement[i][1::,:]]

                ey = np.r_[e, self.crossElement[i][0,:]].astype(int)

                self.qh[i] = PhiFe @ self.s[ey]

            else:
                self.qh[i] = self.PhiFe[i] @ self.s[e]

    def _display_efem(self):
        try:
            cfv.figure()

            # Draw the mesh.

            cfv.drawElementValues(
                ev=self.qh[:,1],
                coords=self.Node,
                edof=self._edof,
                dofs_per_node=self._mesh.dofsPerNode,
                el_type=self._mesh.elType,
                title="EFEM flux field",
                draw_elements=True
                    )
            cfv.showAndWait()
        except Exception as e:
            pass
        

    def efem(self):
        if self._mesh == None:
            print('Please use .meshing() method to create mesh at first!')
        else:
            self._assembly_efem()
            self._constraint()
            self._solve_efem()
            self._construct_flux()
            self._display_efem()

    def fem(self):
        if self._mesh == None:
            print('Please use .meshing() method to create mesh at first!')
        else:
            pass

    def cre(self):
        pass

    def diplay(self):
        pass


# class Poisson2D:
#     """docstring for Poisson
    
#     3-Node triangular element FEM/EFEM model for 2D Poisson/Laplace equation. 

#     """
#     def __init__(self, geometry, DirMarker, NeuMarker, holeMarker, BCfunc):
#         self.Geometry = geometry

#         self.Node = None
#         self.Edge = dict()
#         self.Element = None

#         self.DirMarker = DirMarker # list of markers for Dirichlet's boundary
#         self.Dirichlet = dict()    # (i,j): marker
#         # self.DirEdge = dict()
#         self.NeuMarker = NeuMarker # list of markers for Neumann's boundary
#         self.Neumann = dict()      # (i,j): marker
#         # self.NeuEdge = dict()
#         self.NeuCluster = None
#         self.holeMarker = holeMarker  # list of marker lists for each hole 
#         self.hole = dict()      # (i,j): holeid 
#         self.holeEdge = dict()  # holeid : [(i,j),(m,n),...]

#         self.crossEdge = dict()
#         self.crossElement = dict()

#         self.BCfunc = BCfunc

#         self.numHole = len(self.holeMarker)
#         self.numElement = None
#         self.numNode = None
#         self.numDOF = None

#         self.area = None
#         # EFEM matrix vector
#         self.FU = None
#         self.dU = None
#         self.F = None
#         self.d = None
#         self.PhiFe = None
#         self.s = None
#         self.qh = None
#         # FEM matrix vector

#         self._mesh = None
#         self._edof = None
#         self._BdElements = None
      

#     def mesh(self):
#         # generate mesh using GMSH.
#         self._mesh = cfm.GmshMesh(self.Geometry, return_boundary_elements=True)
#         self._mesh.elType = 2
#         self._mesh.dofsPerNode = 1
#         ##########################################
#         #                                        #
#         #          Basic Data Structure          #
#         #                                        #
#         # Node: list of coordinates (x, y)       #
#         # Element: list of nodes (i,j,k) CCW.    #
#         # Edge: dict of edges (i,j):n pair       #
#         #                                        #
#         ##########################################
#         self.Node, self._edof, _, _, _, self._BdElements = self._mesh.create()
#         self.Element = self._edof - 1
#         for n, e in enumerate(self.Element):
#             self.Edge[(e[0], e[1])] = n
#             self.Edge[(e[1], e[2])] = n
#             self.Edge[(e[2], e[0])] = n
#         self.numElement = len(self.Element)
#         self.numNode = len(self.Node)
#         self.numDOF = self.numNode + self.numHole
#         ###############################################
#         #                                             #
#         #                 Marker Bank                 #
#         #              (for applying BC)              #
#         #                                             #
#         # DirEdge: dict of marker, marker:(i,j)       #
#         # NeuEdge: dict of marker, marker:(i,j)       #
#         # holeEdge: dict of holeid, holeid:[(i,j),..] #
#         #                                             #
#         ###############################################
#         # for marker in self.DirMarker:
#         #     self.DirEdge[marker] = np.array([e['node-number-list'] for e in self._BdElements[marker]])-1
        
#         # for marker in self.NeuMarker:
#         #     self.NeuEdge[marker] = np.array([e['node-number-list'] for e in self._BdElements[marker]])-1

#         for i, hole in enumerate(self.holeMarker):

#             edges = []
#             for marker in hole:
#                 for e in self._BdElements[marker]:
#                     edge = np.array(e['node-number-list'])-1
#                     # Force to CCW.
#                     if edge not in self.Edge:
#                         edge = edge[::-1]
#                     edges.append(edge)
#             self.holeEdge[i] = np.array(edges)

#         ##############################################
#         #                                            #
#         #                Boundary sets               #
#         #          (for applying constraint)         #
#         #                                            #
#         # Dirichlet: Dirichlet edge set.(i,j):marker #
#         # Neumann: Neumann edge set. (i,j):marker    #
#         # hole: hole edge dict. (i,j):holeid         #  
#         #                                            #
#         ##############################################
#         for marker in self.DirMarker:
#             for e in self._BdElements[marker]:
#                 edge = tuple(np.array(e['node-number-list'])-1)
#                 # Force to CCW.
#                 if edge not in self.Edge:
#                     edge = edge[::-1]
#                 self.Dirichlet[edge] = marker

#         for marker in self.NeuMarker:
#             for e in self._BdElements[marker]:
#                 edge = tuple(np.array(e['node-number-list'])-1)
#                 # Force to CCW.
#                 if edge not in self.Edge:
#                     edge = edge[::-1]
#                 self.Neumann[edge] = marker                

#         self.NeuCluster = self._cluster_edge(self.Neumann)

#         self._check_ccw()

#         if len(self.holeMarker) != 0: 
#             for n, hms in enumerate(self.holeMarker):
#                 for m in hms:
#                     for e in self._BdElements[m]:
#                         edge = tuple(np.array(e['node-number-list'])-1)
#                         # Force to CCW.
#                         if edge not in self.Edge:
#                             edge = edge[::-1]
#                         self.hole[edge] = n
#             self._connect_domain()


#     def _cluster_edge(self, edges):
#         from collections import deque
#         from copy import deepcopy

#         edges = deepcopy(edges)
#         clusters = []

#         e, _ = edges.popitem()
#         clusters.append(deque([e]))
        
#         while len(edges) != 0:
#             quit = False
#             for i, j in edges:
#                 for c in clusters:
#                     if i == c[-1][-1]:
#                         c.append((i, j))
#                         edges.pop((i, j), None)
#                         quit = True
#                         break
#                     if j == c[0][0]:
#                         c.appendleft((i, j))
#                         edges.pop((i, j), None)
#                         quit = True
#                         break
#                 if quit:
#                     break
#             else:
#                 if len(edges) != 0:
#                     e, _ = edges.popitem()
#                     clusters.append(deque([e]))

#         return clusters 


#     def _check_ccw(self):
#         import matplotlib.pyplot as plt
#         plt.figure(figsize=(20,10),dpi=300)
#         plt.scatter(self.Node[:,0], self.Node[:,1])
#         for i,j in self.Neumann:
#             x1,y1 = self.Node[i]
#             x2,y2 = self.Node[j]
#             plt.arrow(x1,y2,x2-x1,y2-y1)
#         plt.savefig('ccw.jpg')
#         plt.show()


#     def _connect_domain(self):
#         #################################
#         #                               #
#         #  Crossing elements and edges  #
#         #                               #
#         # crossEdge: (i,j):(n,eta)      #
#         #                               #
#         #################################

#         direction = np.array([0, -1])

#         for n in self.holeEdge:
#             edge = tuple(self.holeEdge[n][0])
#             self.crossEdge[edge] = np.array([[n, -1]])

#             _connected = False

#             while not _connected:
#                 print('current:', edge, self.Node[list(edge)][0], self.Node[list(edge)][1])
#                 ei = self.Edge[edge]
                
#                 i, j, k = self.Element[ei]
#                 order = [i,j,k]  
                
#                 local_edges = set([(i,j), (j,k), (k,i)])
#                 local_edges.remove(edge)
#                 e1 = np.array(local_edges.pop())
#                 e2 = np.array(local_edges.pop())
                
#                 e1n = self.Node[e1[1]] - self.Node[e1[0]]
#                 e1n = e1n/np.linalg.norm(e1n)
                
#                 e2n = self.Node[e2[1]] - self.Node[e2[0]]
#                 e2n = e2n/np.linalg.norm(e2n)
                
#                 d1 = np.abs(np.dot(direction, e1n))
#                 d2 = np.abs(np.dot(direction, e2n))
#                 ae = np.zeros((3,1))
#                 if d1 < d2:
#                     L1 = np.linalg.norm(self.Node[edge[0]] - self.Node[edge[1]])
#                     L2 = np.linalg.norm(self.Node[e1[0]] - self.Node[e1[1]])
#                     ae[order.index(edge[0])]= 1/L1
#                     ae[order.index(e1[0])]= -1/L2
#                     edge = tuple(e1)
#                 else:
#                     L1 = np.linalg.norm(self.Node[edge[0]] - self.Node[edge[1]])
#                     L2 = np.linalg.norm(self.Node[e2[0]] - self.Node[e2[1]])
#                     ae[order.index(edge[0])]= 1/L1
#                     ae[order.index(e2[0])]= -1/L2
#                     edge = tuple(e2)

#                 ae = np.insert(ae, 0, n+self.numNode, axis=0)
#                 if ei in self.crossElement:
#                     self.crossElement[ei] = np.c_[self.crossElement[ei], ae]
#                 else:
#                     self.crossElement[ei] = ae
#                 # print(e1,'\t',e1n)
#                 # print(e2,'\t',e2n)
#                 # print(ae)
#                 # print(d1,d2)
#                 # print("----------------------------------")
#                 print('next:', edge, self.Node[list(edge)][0], self.Node[list(edge)][1])
#                 print(self.Neumann.keys())
#                 if (edge in self.Dirichlet) or (edge in self.Neumann):
#                     if edge in self.hole:
#                         m = self.hole[edge]
#                         if m == n:
#                             # TODO: reject.
#                             pass
#                         else:
#                             if edge in self.crossEdge:
#                                 self.crossEdge[edge] = np.c_[self.crossEdge[edge], np.array([[n+self.numNode], [-1]])]
#                             else:
#                                 self.crossEdge[edge] = np.array([[n+self.numNode], [-1]])
#                     else:
#                         if edge in self.crossEdge:
#                             self.crossEdge[edge] = np.c_[self.crossEdge[edge], np.array([[n+self.numNode], [1]])]
#                         else:
#                             self.crossEdge[edge] = np.array([[n+self.numNode], [1]])

#                     _connected = True
#                 else:
#                     edge = edge[::-1]
#                     # print(self.Node[list(edge)])

#     def _assembly_efem(self):
#         ######################################
#         #                                    #
#         #  unconstrained flexibility matrix  #
#         #                                    #
#         ######################################
#         self.FU = np.zeros((self.numDOF, self.numDOF))
#         self.PhiFe = np.zeros((self.numElement, 2, 3))
#         self.area = np.zeros(self.numElement)

#         for i, e in enumerate(self.Element):
#             x1, y1 = node[e][0]
#             x2, y2 = node[e][1]
#             x3, y3 = node[e][2]
            
#             self.area[i] = 1/2*np.linalg.det(np.array([[x1, y1, 1],
#                                                        [x2, y2, 1],
#                                                        [x3, y3, 1]]))
            
#             self.PhiFe[i] = np.array([[x2-x3, x3-x1, x1-x2],
#                                       [y2-y3, y3-y1, y1-y2]])/(2*self.area[i])
            

#             if i in self.crossElement:
#                 L1 = np.sqrt((x2-x1)**2+(y2-y1)**2)
#                 L2 = np.sqrt((x3-x2)**2+(y3-y2)**2)

#                 left = np.linalg.inv([[(y1-y2)/L1, (x2-x1)/L1],
#                                       [(y2-y3)/L2, (x3-x2)/L2]])

#                 PhiLe = np.c_[left, np.zeros((2,1))]
                
#                 # print(Eh[i])
#                 PhiFe = np.c_[self.PhiFe[i], PhiLe @ self.crossElement[i][1::,:]]
#                 Fe = self.area[i] * PhiFe.T @ PhiFe
                
#                 ### Todo: multiple holes ###
#                 ey = np.r_[e, self.crossElement[i][0,:]]

#                 self.FU[np.ix_(ey,ey)] = self.FU[np.ix_(ey,ey)] + Fe
#             else:
#                 Fe = self.area[i] * self.PhiFe[i].T @ self.PhiFe[i]
                
#                 self.FU[np.ix_(e,e)] = self.FU[np.ix_(e,e)] + Fe

#         print('size: {} x {}'.format(*self.FU.shape))
#         print('rank: {}'.format(np.linalg.matrix_rank(self.FU)))

#         ######################################
#         #                                    #
#         #   unconstrained kinematic vector   #
#         #                                    #
#         ######################################
#         self.dU = np.zeros(self.numDOF)
#         #========================#
#         #   Dirichlet boundary   #
#         #========================#
#         for edge in self.Dirichlet:
#             marker = self.Dirichlet[edge]
#             i, j = edge
#             L = np.linalg.norm(self.Node[i]-self.Node[j])
#             up = gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
#             self.dU[i] = self.dU[i] - up/L
#             self.dU[j] = self.dU[j] + up/L

#         # for marker in self.DirEdge:
#         #     for i,j in self.DirEdge[marker]:
#         #         L = np.linalg.norm(self.Node[i]-self.Node[j])
#         #         up = gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])
#         #         self.dU[i] = self.dU[i] - up/L
#         #         self.dU[j] = self.dU[j] + up/L


#     def _constraint(self):
#         #========================================#
#         #    Neumann boundary (without source)   #
#         #========================================#
#         self.sN = np.zeros(self.numDOF)
#         self.A = np.eye(self.numDOF)

#         # todo : move to subfunction.
#         eff_dofs = []
#         red_dofs = []
#         for c in self.NeuCluster:
#             eff_dofs.append([c[0][0]])
#             _red = []
#             for e in c:
#                 # to avoid error occured in closed-edge.
#                 if e[1] != eff_dofs[-1][0]:
#                     _red.append(e[1])
#             red_dofs.append(_red)

#         for n, d in enumerate(eff_dofs):
#             self.A[np.ix_(red_dofs[n], d)] = 1

#         all_red_dofs = [y for x in red_dofs for y in x]

#         # Neumann terms
#         for c in self.NeuCluster:
#             rn = {}
#             fl_bar = 0
#             # exclude the last edge in closed edge.
#             if c[0][0] == c[-1][-1]:
#                 cn = c[:-1]
#             else:
#                 cn = c

#             for e in cn:
#                 marker = self.Neumann[e]
#                 i, j = e    
#                 fl_bar = fl_bar + gauss_line_int(self.BCfunc[marker], *node[i], *node[j])
#                 self.sN[j] = fl_bar

#                 if e in self.crossEdge:
#                     dofs, etas = self.crossEdge[e]
#                     for dof, eta in zip(dofs, etas):
#                         rn[dof] = eta

#                 for dof in rn:
#                     self.A[j, dof] = rn[dof]

#         self.A = np.delete(self.A, all_red_dofs, axis=1)

#         # rk 

#         FU = self.FU[:]
#         dU = self.dU[:]

#         for c in self.NeuCluster:
#             # 封闭的一定是孔吗？
#             if c[0][0] == c[-1][-1]:
#                 n = self.hole[c[0]]
#                 dof = n + self.numNode
#                 r = 0
#                 for e in c:
#                     marker = self.Neumann[e]
#                     r = r + gauss_line_int(self.BCfunc[marker], *self.Node[i], *self.Node[j])

#                 FU[dof,:] = 0
#                 FU[dof,dof] = 1
#                 dU[dof] = r

#         # r1 = 0
#         FU[0,:] = 0
#         FU[0,0] = 1
#         dU[0] = 0

#         self.F = self.A.T @ self.F @ self.A
#         self.d = self.A.T @ (self.d - self.F @ self.sN)


#     def _solve_efem(self):
#         s = np.linalg.inv(self.F) @ self.d
#         self.s = self.A @ s + self.sN

#     def _construct_flux(self):
#         self.qh = np.zeros((self.numElement, 2)) 

#         for i, e in enumerate(self.Element):
            
#             if i in self.crossElement:
#                 x1,y1 = node[e][0]
#                 x2,y2 = node[e][1]
#                 x3,y3 = node[e][2]
#                 L1 = np.sqrt((x2-x1)**2+(y2-y1)**2)
#                 L2 = np.sqrt((x3-x2)**2+(y3-y2)**2)

#                 left = np.linalg.inv([[(y1-y2)/L1,(x2-x1)/L1],
#                                       [(y2-y3)/L2,(x3-x2)/L2]])

#                 PhiLe = np.c_[left, np.zeros((2,1))]
                
#                 PhiFe = np.c_[self.PhiFe[i], PhiLe @ self.crossElement[i][1::,:]]

#                 ey = np.r_[e, self.crossElement[i][0,:]]

#                 self.qh[i] = PhiFe @ self.s[ey]

#             else:
#                 self.qh[i] = self.PhiFe[i] @ self.s[e]

#     def _display_efem(self):
#         cfv.figure()

#         # Draw the mesh.

#         cfv.drawElementValues(
#             ev=self.qh[:,1],
#             coords=self.Node,
#             edof=self._edof,
#             dofs_per_node=self._mesh.dofsPerNode,
#             el_type=self._mesh.elType,
#             title="EFEM flux field",
#             draw_elements=False
#                 )
#         cfv.showAndWait()


#     def efem():
#         if self._mesh == None:
#             print('Please use .meshing() method to create mesh at first!')
#         else:
#             self._assembly_efem()
#             self._constraint()
#             self._solve_efem()
#             self._construct_flux()
#             self._display_efem()

#     def fem():
#         if self._mesh == None:
#             print('Please use .meshing() method to create mesh at first!')
#         else:
#             pass

#     def cre():
#         pass

#     def diplay():
#         pass


def create_geometry(el_on_curve=5):
    # Geometry definition
    g = cfg.Geometry()

    r1 = 0.15
    r2 = r1/np.sqrt(2)

    points = [[0,1],[0.4,1],[0.6,1],[0.8,1],[1.2,1],[1.4,1],[1.6,1],[2.0,1],  # 0-7
              [0,0.7],[0.4,0.7],[0.6,0.7],[0.8,0.7],[1.2,0.7],[1.4,0.7],[1.6,0.7],[2.0,0.7], # 8-15
              [0,0.5],[0.4,0.5],[0.6-r1,0.5],[0.6-r2,0.5+r2],[0.6,0.5+r1],[0.6+r2,0.5+r2],[0.6+r1,0.5],[0.8,0.5], # 16-23
              [1.2,0.5],[1.4-r1,0.5],[1.4-r2,0.5+r2],[1.4,0.5+r1],[1.4+r2,0.5+r2],[1.4+r1,0.5],[1.6,0.5],[2,0.5], # 24-31
              [0.6,0.5],[1.4,0.5], # 32-33 circle center
              [0.6-r2,0.5-r2],[0.6,0.5-r1],[0.6+r2,0.5-r2],[1.4-r2,0.5-r2],[1.4,0.5-r1],[1.4+r2,0.5-r2], # 34-39
              [0,0.3],[0.4,0.3],[0.6,0.3],[0.8,0.3],[1.2,0.3],[1.4,0.3],[1.6,0.3],[2,0.3], # 40-47
              [0,0],[0.4,0],[0.6,0],[0.8,0],[1.2,0],[1.4,0],[1.6,0],[2,0]] # 48-55

    splines = [[0,1],[1,2],[2,3],[3,4],[4,5],[5,6],[6,7], # 0-6 top
               [0,8],[1,9],[2,10],[3,11],[4,12],[5,13],[6,14],[7,15], # 7-14
               [8,9],[9,10],[10,11],[11,12],[12,13],[13,14],[14,15], # 15-21
               [8,16],[9,17],[9,19],[10,20],[11,21],[11,23],[12,24],[12,26],[13,27],[14,28],[14,30],[15,31], # 22-33
               [16,17],[17,18],[22,23],[23,24],[24,25],[29,30],[30,31], #34-40
               [16,40],[17,41],[34,41],[35,42],[36,43],[23,43],[24,44],[37,44],[38,45],[39,46],[30,46],[31,47], #41-52
               [40,41],[41,42],[42,43],[43,44],[44,45],[45,46],[46,47], # 53-59
               [40,48],[41,49],[42,50],[43,51],[44,52],[45,53],[46,54],[47,55],# 60-67
               [48,49],[49,50],[50,51],[51,52],[52,53],[53,54],[54,55],# 68-74
              ]

    longs = [0,3,6,15,18,21,34,37,40,53,56,59,68,71,74]

    arcs = [[18,32,19],[19,32,20],[20,32,21],[21,32,22],
            [22,32,36],[36,32,35],[35,32,34],[34,32,18], # 75-82
            [25,33,26],[26,33,27],[27,33,28],[28,33,29],
            [29,33,39],[39,33,38],[38,33,37],[37,33,25], # 83-90
           ]

    surfaces = [[0,8,15,7],[1,9,16,8],[2,10,17,9],[3,11,18,10],[4,12,19,11],[5,13,20,12],[6,14,21,13],
             [15,23,34,22],[24,75,35,23],[16,25,76,24],[17,26,77,25],[26,27,36,78],
             [18,28,37,27],
             [29,83,38,28],[19,30,84,29],[20,31,85,30],[31,32,39,86],[21,33,40,32],
             [34,42,53,41],[35,82,43,42],[81,44,54,43],[80,45,55,44],[36,46,45,79],
             [37,47,56,46],
             [38,90,48,47],[89,49,57,48],[88,50,58,49],[39,51,50,87],[40,52,59,51],
             [53,61,68,60],[54,62,69,61],[55,63,70,62],[56,64,71,63],[57,65,72,64],[58,66,73,65],[59,67,74,66]]

    for x, y in points:
        g.point([x, y])

    for i, s in enumerate(splines):
        if i in longs:
            g.spline(s, el_on_curve=2*el_on_curve)
        else:
            g.spline(s, el_on_curve=el_on_curve)

    for c in arcs:
        g.circle(c, el_on_curve=el_on_curve)

    for f in surfaces:
        g.struct_surf(f)


    bcs = {100:[7,22,41,60],
           200:[68,69,70,71,72,73,74],
           300:[75,76,77,78],
           101:[0,1,2,3,4,5,6],
           201:[14,33,52,67],
           301:[79,80,81,82],
           401:[83,84,85,86,87,88,89,90],
           }

    for marker in bcs:
        for i in bcs[marker]:
            g.curve_marker(ID=i, marker=marker)

    return g

def dir100(x,y):
    return np.zeros_like(x)

def dir200(x,y):
    return np.zeros_like(x)

def dir300(x,y):
    return np.ones_like(x)

def neu101(x,y):
    return np.ones_like(x)

def neu201(x,y):
    return np.ones_like(x)

def neu301(x,y):
    return np.ones_like(x)

def neu401(x,y):
    return np.ones_like(x)

def main():
    geometry = create_geometry(el_on_curve=5)
    Dirichlet = [100,200,300]
    Neumann = [101,201,301,401]
    hole = [[300,301],[401]]

    BCfunc = {100:dir100, 200:dir200, 300:dir300,
              101:neu101, 201:neu201, 301:neu301, 401:neu401}


    model = Poisson2D(geometry, Dirichlet, Neumann, hole, BCfunc)
    model.mesh()
    model.efem()
    # model.fem()

    # model.display()



if __name__ == '__main__':
    main()